#include "astar.h"

#include <QDebug>

Node2D popNode2D(vector<Node2D> &O)
{
    float value = 10000;//足够大，大于可能的启发距离
    int index = 0;
    for (int i = 0; i < O.size(); i++)
    {
        float f = O[i].g + O[i].h;
        if (f < value) {
            value = f;
            index = i;
        }
    }
    Node2D output = O[index];
    O.erase(O.begin() + index);
    return output;
}
//bool Astar(QVector<QVector<int>> map, int startx, int starty, int endx, int endy, vector<int> &pathx, vector<int> &pathy,vector<int> &stepx, vector<int> &stepy)
bool Astar(QVector<QVector<int>> map, int startx, int starty, int endx, int endy, vector<int> &pathx, vector<int> &pathy)
{
    int height, width;
    //获得map尺寸
    height=map.size();
    width=map[0].size();
    //map二维数组，1为障碍物，0为非障碍物
    qDebug()<<"startx:"<<startx<<";starty:"<<starty<<"endx:"<<endx<<";endy:"<<endy;
    qDebug()<<"width:"<<width<<";height:"<<height;
    if (startx < 0 || startx >= width || starty < 0 || starty >= height || endx < 0 || endx >= width || endy < 0 || endy >= height)
        return false;       //起点或终点在map以外
    if (map[starty][startx] == 1 || map[endy][endx] == 1)//起点或终点是障碍物
        return false;
    qDebug()<<"width:"<<width<<";height:"<<height;

    int directions[8][2] = { { -1, 1 },{ 0, 1 },{ 1, 1 },{ -1, 0 },{ 1, 0 },{ -1, -1 },{ 0, -1 },{ 1, -1 } };//八邻域搜索方向
    short *isVisited = new short[height*width];//表示节点状态，0:未访问，1：open,2:close
    for (int i = 0; i < height*width; i++)
        isVisited[i] = 0;

    Node2D* nodes2d = new Node2D[width*height];//存储所有节点

    Node2D startNode, endNode;//初始化起点和终点
    startNode.x = startx;
    startNode.y = starty;
    startNode.g = 0;
    startNode.h = abs(startNode.x - endx) + abs(startNode.y - endy);//使用曼哈顿距离
    startNode.f = startNode.g + startNode.h;
    startNode.idx = starty*width + startx;
    startNode.preidx = -1;//起点没有父节点

    endNode.x = endx;
    endNode.y = endy;


    Node2D nPred;//当前节点
    Node2D nSucc;//子节点
    int iPred, iSucc;//节点id

    vector<Node2D> O;//开列表
    vector<Node2D> C;//闭列表
    O.push_back(startNode);         //将startNode 压入O的最后一个单元
    isVisited[startNode.idx] = 1;
    nodes2d[startNode.idx] = startNode;

    while (!O.empty())
    {
        nPred = popNode2D(O);   //获取开列表中的最小距离节点作为当前节点,此节点将进行邻域
        iPred = nPred.y * width + nPred.x;

//        stepx.push_back(nPred.x);
//        stepy.push_back(nPred.y);

        if (nPred.x == endNode.x && nPred.y == endNode.y)
        {
            //反向生成路径
            Node2D tempNode = nPred;
            while (tempNode.preidx > -1)
            {
                pathx.push_back(tempNode.x);
                pathy.push_back(tempNode.y);
                tempNode = nodes2d[tempNode.preidx];
            }
            pathx.push_back(tempNode.x);
            pathy.push_back(tempNode.y);

            int start = 0;
            int end = pathx.size()-1;
            while (start < end)//反转一下顺序
            {
                swap(pathx[start], pathx[end]);
                swap(pathy[start], pathy[end]);
                start++;
                end--;
            }
            delete[]isVisited;
            delete[]nodes2d;
            return true;
        }

        isVisited[iPred] = 2;//更新为close状态
        C.push_back(nodes2d[iPred]);


        for (char i = 0; i < 8; i++)
        {
            //八邻域检测
            int xSucc = nPred.x + directions[i][0]; //获得邻域坐标
            int ySucc = nPred.y + directions[i][1];
            if (xSucc >= 0 && xSucc < width && ySucc >= 0 && ySucc < height && (map[ySucc][xSucc] != 1 && isVisited[ySucc*width + xSucc] != 2))
                //边界检测、非障碍物、不在closed链表中
            {
                nSucc.x = xSucc;
                nSucc.y = ySucc;
                nSucc.idx = ySucc*width + xSucc;
                nSucc.preidx = nPred.idx;
                nSucc.g = nPred.g + sqrt((xSucc - nPred.x) * (xSucc - nPred.x) + (ySucc - nPred.y) * (ySucc - nPred.y));
                nSucc.h = abs(nSucc.x - endNode.x) + abs(nSucc.y - endNode.y);
                nSucc.f = nSucc.g + nSucc.h;
                iSucc = nSucc.y * width + nSucc.x;

                if (isVisited[iSucc] == 0)//第一次访问
                {
                    isVisited[iSucc] = 1;//更新状态
                    nodes2d[iSucc] = nSucc;
                    O.push_back(nSucc);//插入Open
                }
                else if (isVisited[iSucc] == 1 && nSucc.f + 0.00001 < nodes2d[iSucc].f)//处于open表中
                {
                    nodes2d[iSucc] = nSucc;
                    for (int i = 0; i < O.size(); i++) //查找Open表中的该点并替换
                    {
                        if (O[i].x == nSucc.x && O[i].y == nSucc.y) {
                            O[i] = nSucc;//更新Open
                            break;
                        }
                    }
                }
            }
        }

    }
    delete[]isVisited;
    delete[]nodes2d;
    return false;
}
